/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file capabilitiesManager.cxx
  \brief class implemenation XML parser

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>

#include "capabilitiesManager.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
extern String vehicleID;
/*<-End Global Variables-->*/

CapabilitiesManager::CapabilitiesManager() 
{
	for(int i = 0; i<MAX_NUM_CAP; i++) {
		capabilities.systems[i] = NULL;
	}
}

CapabilitiesManager::~CapabilitiesManager() 
{
	for(int i = 0; i<MAX_NUM_CAP; i++) {
		if(capabilities.systems[i] != NULL) {
			delete capabilities.systems[i];
			capabilities.systems[i] = NULL;
		}
	}
}

bool CapabilitiesManager::add(SystemCharacter_t* a_system) 
{
	if(find(a_system)==0) {
		for(int i = 0; i<MAX_NUM_CAP; i++) {
			if(capabilities.systems[i] == NULL) {
				capabilities.systems[i] = new SystemCharacter_t; 
				memcpy(capabilities.systems[i], a_system, sizeof(SystemCharacter_t));
				return true;
			}
		}
	} else {
		for(int i = 0; i<MAX_NUM_CAP; i++) {
			if((capabilities.systems[i])->id == a_system->id) {
				(capabilities.systems[i])->commands |= a_system->commands;
				(capabilities.systems[i])->streams |= a_system->streams;
				return true;
			}
		}
	}
	return false;
}

bool CapabilitiesManager::remove(SystemCharacter_t* a_system) 
{
	if(a_system == NULL ) {
		for(int i = 0; i<MAX_NUM_CAP; i++) {
			if( capabilities.systems[i] )
				delete capabilities.systems[i];
			capabilities.systems[i] = NULL;
		}
		return true;
	}
	else {
		int i;
		if((i = find(a_system)) > 0) {
			capabilities.systems[i-1] = NULL;
			return true;
		}
	}
	return false;
}

void CapabilitiesManager::getCapabilities(uint8_t * buf)
{
	char * dataPtr = (char *)CMD_DATAPTR(buf);
	char * tmpPtr = dataPtr;

	CMD_CMD(buf) = CMD_CAPABILITY;
	strncpy(dataPtr, vehicleID.c_str(), 31);
	dataPtr[31] = '\0'; // saftey check to make sure name ends in null char if too big
	tmpPtr += 32;
	for(int i = 0; i<MAX_NUM_CAP; i++)
		if(capabilities.systems[i] != NULL) {
			memcpy(tmpPtr, capabilities.systems[i], sizeof(SystemCharacter_t));
			tmpPtr += sizeof(SystemCharacter_t);
		}
	CMD_NUM(buf)  = (tmpPtr - dataPtr);
	CMD_CSUM(buf) = 0;
	CMD_CSUM(buf) = C_calcCheckSum(buf);
}

void CapabilitiesManager::printCapabilities(void) 
{
	for(int i = 0; i<MAX_NUM_CAP; i++)
		if(capabilities.systems[i] != NULL) {
			cout << "        |-> [" << hex <<  (int)(capabilities.systems[i]->id) << "] " << dec;

			for(int j=0; j< 16; j++) {
				if(((capabilities.systems[i]->commands) >> j) & 0x1)
					cout << j+1 << " ";
			}
			cout << " :  ";
			for(int j=0; j< 16; j++) {
				if(((capabilities.systems[i]->streams) >> j) & 0x1)
					cout << j+1 << " ";
			}
			cout << endl;
		}
}

bool CapabilitiesManager::find(uint8_t a_system_id) 
{
	for(int i = 0; i<MAX_NUM_CAP; i++)
		if(capabilities.systems[i] != NULL) {
			if( (capabilities.systems[i]->id) == (a_system_id) )
				return true;
			else {
				if(((capabilities.systems[i]->id) & 0xF0) == ((a_system_id) & 0xF0)) {
					char subsys = ((a_system_id) & 0x0F) - 1;
					if(((capabilities.systems[i]->commands) >> subsys) & 0x1)
						return true;
					if(((capabilities.systems[i]->streams) >> subsys) & 0x1)
						return true;
				}
			}
		}
	return false;
}

int CapabilitiesManager::find(SystemCharacter_t* a_system) 
{
	if( !a_system )
		return -1;

	for(int i = 0; i<MAX_NUM_CAP; i++)
		if(capabilities.systems[i] != NULL) {
			if((capabilities.systems[i]->id) == (a_system->id))
				return i+1;
		}
	return 0;
}

SystemCharacter_t * CapabilitiesManager::get(uint8_t num) 
{
	if( num >= MAX_NUM_CAP )
		return NULL;

	return capabilities.systems[num];
}
